#include "commWithBoard.h"
#include <QDebug>
#include <cstring>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>

BoardRecvThread::BoardRecvThread(QObject *parent) : QThread(parent) {
    struct mq_attr attr;
    attr.mq_flags = 0;
    attr.mq_maxmsg = 10;
    attr.mq_msgsize = MAX_MSG_SIZE;
    attr.mq_curmsgs = 0;

    mq_ = mq_open(QUEUE_IO_QT, O_RDONLY | O_CREAT, 0644, &attr);
    if (mq_ == (mqd_t)-1) {
        qCritical() << "[BoardRecvThread] Failed to open message queue.";
    }
}

BoardRecvThread::~BoardRecvThread() {
    running_ = false;
    if (mq_ != (mqd_t)-1) {
        mq_close(mq_);
        mq_unlink(QUEUE_IO_QT); // 仅由拥有创建权的一方调用，否则注释掉
    }
}

void BoardRecvThread::run() {
    char buffer[MAX_MSG_SIZE];
    while (running_) {
        memset(buffer, 0, sizeof(buffer));
        ssize_t bytes = mq_receive(mq_, buffer, MAX_MSG_SIZE, nullptr);
        if (bytes > 0) {
            QString msg = QString::fromUtf8(buffer, bytes);
            emit messageReceived(msg);
        } else {
            msleep(10); // 防止 busy loop
        }
    }
}

commWithBoard::commWithBoard(QObject *parent) : QObject(parent)
{
    boardSigToBridge = new CommSignal(this);
    recvThread_ = new BoardRecvThread(this);

    connect(recvThread_, &BoardRecvThread::messageReceived,
            this, &commWithBoard::onMessageReceived);

    recvThread_->start();
}

commWithBoard::~commWithBoard()
{
    qDebug() << "~commWithBoard is called";
    recvThread_->quit();
    recvThread_->wait();
    delete recvThread_;
    delete boardSigToBridge;
}

void commWithBoard::onMessageReceived(const QString &msg) 
{
    qDebug() << "[commWithBoard] Received:" << msg;
    if(msg.startsWith("START"))
    {
        qDebug() << "Send START";
        boardSigToBridge->send(GUI_SIG_START);
    }
    else if(msg.startsWith("STOP"))
    {
        boardSigToBridge->send(GUI_SIG_STOP);
    }
    else if(msg.startsWith("PAUSE"))
    {
        boardSigToBridge->send(GUI_SIG_PAUSE);
    }
    else if(msg.startsWith("GRIPPER_OPEN_L"))
    {
        boardSigToBridge->send(GUI_SIG_GRIPPER_OPEN_L);
    }
    else if(msg.startsWith("GRIPPER_CLOSE_L"))
    {   
        boardSigToBridge->send(GUI_SIG_GRIPPER_CLOSE_L);
    }
    else if(msg.startsWith("GRIPPER_OPEN_R"))
    {
        boardSigToBridge->send(GUI_SIG_GRIPPER_OPEN_R);
    }
    else if(msg.startsWith("GRIPPER_CLOSE_R"))
    {   
        boardSigToBridge->send(GUI_SIG_GRIPPER_CLOSE_R);
    }
    else if(msg.startsWith("MOVE_F"))
    {
        boardSigToBridge->send(GUI_SIG_MOVE_F);
    }    
    else if(msg.startsWith("MOVE_B"))
    {
        boardSigToBridge->send(GUI_SIG_MOVE_B);
    }
    else if(msg.startsWith("MOVE_L"))
    {
        boardSigToBridge->send(GUI_SIG_MOVE_L);
    }
    else if(msg.startsWith("MOVE_R"))
    {
        boardSigToBridge->send(GUI_SIG_MOVE_R);
    }
}